/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_Studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#include "stdafx.h"
#include "TDtree.h"




// 创建TD树节点
S_TDNode* f_createTDNode(vec3 data) {
	S_TDNode* node = (S_TDNode*)malloc(sizeof(S_TDNode));
	if (node != nullptr) {
		node->data = data;
		node->leftChild = nullptr;
		node->rightChild = nullptr;
	}
	return node;
}

// 释放TD树节点及其子节点的内存
void f_destroyTDNode(S_TDNode* node) {
	if (node != nullptr) {
		f_destroyTDNode(node->leftChild);
		f_destroyTDNode(node->rightChild);
		free(node);
	}
}

// 在TD树中插入节点
S_TDNode* f_insertTDNode(S_TDNode** root, vec3 data, int depth) {
	if (*root == nullptr) {
		*root = f_createTDNode(data);
		return *root;
	}

	int axis = depth % 3; // 根据深度选择划分轴
	S_TDNode** child = nullptr; //(axis == 0) ? &((*root)->leftChild) : &((*root)->rightChild);

	// 根据坐标值比较，递归插入节点
	if (axis == 0) {
		if (data.x < (*root)->data.x)
			child = &((*root)->leftChild);
		else
			child = &((*root)->rightChild);
	}
	else if (axis == 1) {
		if (data.y < (*root)->data.y)
			child = &((*root)->leftChild);
		else
			child = &((*root)->rightChild);
	}
	else {
		if (data.z < (*root)->data.z)
			child = &((*root)->leftChild);
		else
			child = &((*root)->rightChild);
	}


	if (*child == nullptr) {
		*child = f_createTDNode(data);
		return *child;
	}
	else {
		return f_insertTDNode(child, data, depth + 1);
	}

	//if (axis == 0) {
	//	if (data.x < (*child)->data.x)
	//		(*child)->leftChild = f_insertTDNode(child, data, depth + 1);
	//	else
	//		(*child)->rightChild = f_insertTDNode(child, data, depth + 1);
	//}
	//else if (axis == 1) {
	//	if (data.y < (*child)->data.y)
	//		(*child)->leftChild = f_insertTDNode(child, data, depth + 1);
	//	else
	//		(*child)->rightChild = f_insertTDNode(child, data, depth + 1);
	//}
	//else {
	//	if (data.z < (*child)->data.z)
	//		(*child)->leftChild = f_insertTDNode(child, data, depth + 1);
	//	else
	//		(*child)->rightChild = f_insertTDNode(child, data, depth + 1);
	//}

	//return child;
	//if ((axis == 0 && data.x < (*child)->data.x) || (axis != 0 && data.y < (*child)->data.y))
	//	return f_insertTDNode(child, data, depth + 1);
	//else
	//	return f_insertTDNode(child, data, depth + 1);
}

// 判断两个点的距离是否小于等于给定容差
bool f_isWithinTolerance(vec3 p1, vec3 p2, double tolerance) {
	return vec_len(p1 - p2) <= tolerance;
	//double distanceSquared = pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2);
	//return distanceSquared <= pow(tolerance, 2);
}

// 在TD树中查找指定的节点
bool f_findTDNode(S_TDNode* root, vec3 targetPoint, double tolerance, int depth) {
	if (root == nullptr) return false;

	// 如果当前节点的数据与目标点接近，则返回true
	if (f_isWithinTolerance(root->data, targetPoint, tolerance))
		return true;

	int axis = depth % 3; // 根据深度选择划分轴

	// 根据坐标值比较，递归查找节点
	if ((axis == 0 && targetPoint.x < root->data.x) || (axis != 0 && targetPoint.y < root->data.y))
		return f_findTDNode(root->leftChild, targetPoint, tolerance, depth + 1);
	else
		return f_findTDNode(root->rightChild, targetPoint, tolerance, depth + 1);
}

// 判断一个盒子是否包含在另一个盒子中
//bool f_basic_isBoxContained(vec3 center, vec3 boxLength, vec3 target, double tolerance) {
//	vec3 halfLength = boxLength / 2;
//
//	// 确定XYZ轴范围
//	double xMin = center.x - halfLength.x;
//	double xMax = center.x + halfLength.x;
//	double yMin = center.y - halfLength.y;
//	double yMax = center.y + halfLength.y;
//	double zMin = center.z - halfLength.z;
//	double zMax = center.z + halfLength.z;
//
//	// 判断目标点是否在容差范围内，并且在盒子的XYZ范围内
//	return target.x >= xMin && target.x <= xMax &&
//		target.y >= yMin && target.y <= yMax && target.z >= zMin && target.z <= zMax;
//	//return fabs(target.x - center.x) <= tolerance && fabs(target.y - center.y) <= tolerance &&
//	//	fabs(target.z - center.z) <= tolerance && target.x >= xMin && target.x <= xMax &&
//	//	target.y >= yMin && target.y <= yMax && target.z >= zMin && target.z <= zMax;
//}

// 在TD树中查找指定范围内的节点
S_TDNodeArray* f_rangeSearch(S_TDNode* root, vec3 center, vec3 boxLength, double tolerance, int depth) {
	if (root == NULL)
		return nullptr;

	int axis = depth % 3; // 根据深度选择划分轴

	S_TDNodeArray* ta = (S_TDNodeArray*)f_core_array_new(1, sizeof(S_TDNode*));
	// 判断当前节点是否在给定范围内
	if (f_basic_isBoxContained(center, boxLength, root->data)) {
		printf("Node at (%.2f, %.2f, %.2f) is within the range.\n", root->data.x, root->data.y, root->data.z);
		
		ta->ptr_userData[0] = root;
	}
	else {
		ta->ptr_userData[0] = nullptr;
		ta->count = 0;
	}
		

	// 根据坐标值比较，判断和子节点递归查找
	//if ((axis == 0 && center.x - tolerance <= root->data.x) || (axis != 0 && center.y - tolerance <= root->data.y)) {
	//	auto* a = f_rangeSearch(root->leftChild, center, boxLength, tolerance, depth + 1);
	//	f_core_array_append((S_Array*)ta, (S_Array*)a);
	//}
	if (root->leftChild) {
		auto* a = f_rangeSearch(root->leftChild, center, boxLength, tolerance, depth + 1);
		f_core_array_append((S_Array*)ta, (S_Array*)a);
		if(a ) f_core_array_free((S_Array*)a);
	}
		
	//if ((axis == 0 && center.x + tolerance >= root->data.x) || (axis != 0 && center.y + tolerance >= root->data.y)) {
	//	auto* a = f_rangeSearch(root->rightChild, center, boxLength, tolerance, depth + 1);
	//	f_core_array_append((S_Array*)ta, (S_Array*)a);
	//}
	if (root->rightChild) {
		auto* a = f_rangeSearch(root->rightChild, center, boxLength, tolerance, depth + 1);
		f_core_array_append((S_Array*)ta, (S_Array*)a);
		if (a) f_core_array_free((S_Array*)a);
	}
	return ta;
}

















